#!/usr/bin/python3

import serial
import time
from datetime import datetime

serialHandle = serial.Serial("com6", 115200)  # 初始化串口， 波特率为115200

command = {"MOVE_WRITE": 1, "POS_READ": 28, "LOAD_UNLOAD_WRITE": 31}


##
##命令发送
##
def servoWriteCmd(id, cmd, position=None, runtime=None):
    buf = bytearray(b'\x55\x55')
    try:
        len = 3  # 若命令是没有参数的话数据长度就是3
        buf1 = bytearray(b'')

        ## 对参数进行处理
        if position is not None:
            len += 2  # 数据长度加2
            buf1.extend([(0xff & position), (0xff & (position >> 8))])  # 分低8位 高8位 放入缓存
            # print("position = ", buf1.hex())
            ## 1111 1111&0000 0001 = 0000 0001, 0000 0000
        if runtime is not None:
            len += 2
            buf1.extend([(0xff & runtime), (0xff & (runtime >> 8))])  # 分低8位 高8位 放入缓存
            # print("runtime= ", buf1.hex())

        # print("buf1 =", buf1.hex())
        buf.extend([(0xff & id), (0xff & len), (0xff & cmd)])

        # print("buf=", buf.hex())
        buf.extend(buf1)  # 追加参数

        ##计算校验和
        sum = 0x00
        for b in buf:  # 求和
            sum += b
        sum = sum - 0x55 - 0x55  # 去掉命令开头的两个 0x55
        sum = ~sum  # 取反
        buf.append(0xff & sum)  # 取低8位追加进缓存
        # print("buf == ", buf.hex())
        serialHandle.write(buf)  # 发送
    except Exception as e:
        print(e)


##
##读取位置
##
def readPosition(id):
    serialHandle.flushInput()  # 清空接收缓存
    servoWriteCmd(id, command["POS_READ"])  # 发送读取位置命令
    time.sleep(0.00034)  # 小延时，等命令发送完毕。不知道是否能进行这么精确的延时的，但是修改这个值的确实会产生影响。
    # 实验测试调到这个值的时候效果最好
    time.sleep(0.004)  # 稍作延时，等待接收完毕
    count = serialHandle.inWaiting()  # 获取接收缓存中的字节数
    pos = None
    if count != 0:  # 如果接收到的数据不空
        recv_data = serialHandle.read(count)  # 读取接收到的数据
        if count == 8:  # 如果接收到的数据是8个字节（符合位置读取命令的返回数据的长度）
            if recv_data[0] == 0x55 and recv_data[1] == 0x55 and recv_data[4] == 0x1C:
                # print("id = ", recv_data[2])
                # print("len = ", recv_data[3])

                # 第一第二个字节等于0x55, 第5个字节是0x1C 就是 28 就是 位置读取命令的命令号
                pos = 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8)))  # 将接收到的字节数据拼接成完整的位置数据
                # 上面这小段代码我们简化了操作没有进行和校验，只要帧头和命令对了就认为数据无误

    return pos  # 返回读取到的位置


while True:
    try:
        t = 10000;
        servoWriteCmd(1, command["MOVE_WRITE"], 0, t)  # 发送命令 参数1 舵机id=1, 参数2 命令 = 1, 参数3 位置 = 0, 参数4 时间 = 1000ms
        endtime = datetime.now().timestamp() + t / 1000;
        lasttime = datetime.now().timestamp()
        nowtime = lasttime

        position = readPosition(1)
        lastposition = position
        i = 0;
        print("start time ==", datetime.now())
        while (nowtime < endtime + 1):
            position = readPosition(1)
            # print(datetime.now(), position, lastposition, sep="  ==  ")
            if abs(position - lastposition) == 1:
                # print(datetime.now(), "  =====  ", position)
                lasttime = nowtime
                nowtime = datetime.now().timestamp()
                print("和上次执行时间间隔 ==", nowtime - lasttime, "  上次位置", lastposition,  "当前位置", position)
            lastposition = position
            i = i + 1
        print("执行完成，总共获取位置次数  ", i)
        print("end time ==", datetime.now())
        servoWriteCmd(1, command["MOVE_WRITE"], 100, 3000)  # 发送命令 参数1 舵机id=1, 参数2 命令 = 1, 参数3 位置 = 0, 参数4 时间 = 1000ms
        time.sleep(10.1)
    except Exception as e:
        print(e)
        break

## 协议头 0x55 0x55 (协议头不计入len 长度)
## 1 byte 是 舵机ID
## 2 byte 是 数据len
## 3 byte 是 命令 1 代表转动舵机

## 后面的位数代表 第三byte位的参数
